李雅普诺夫公式 期望位姿 轨迹误差 动力学